#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"

class MotorController : public rclcpp::Node
{
public:
    MotorController() : Node("motor_controller")
    {
        this->twist_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "cmd_vel", 10,
            std::bind(&MotorController::twistCallback, this, std::placeholders::_1));
    }

private:
    void twistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) const
    {
      float x = msg->linear.x;
      float y = msg->linear.y;
      float z = msg->angular.z;
      RCLCPP_INFO(this->get_logger(),"x:%f,y:%f,z:%f",x,y,z);
    }


    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MotorController>());
    rclcpp::shutdown();
    return 0;
}